# 导入必要的库和模块
import rospy
import math
from action_t import Action_t
import time

drone = Action_t("real_nolog")

flag = 1


# 主循环
while True:
    # 状态机处理（修改每个状态后的flag跳转逻辑）
    if flag == 1:  # 解锁状态
        print("状态1: 解锁无人机")
        if not drone.unlock_thread_running:
            drone.unlock(use_thread=True)
        if drone.control_complete():
            flag = 2

    elif flag == 2 :
        print("状态2: 发送位置目标，无人机起飞")
        drone.send_position_x_y_z_t(0, 0, 1.2, 4, use_thread=True)
        if drone.control_complete():
            flag = 3

    elif flag == 3 :
        print("状态3: 发送导航，无人机往前飞")
        drone.publish_nav_goal_x_y_z_yaw_t_frame(-1, 1, 1.2, 90, 6, 1, use_thread=True)
        if drone.control_complete():
            flag = 4

    elif flag == 4 :
        print("状态3: 发送导航，无人机往前飞")
        drone.publish_nav_goal_x_y_z_yaw_t_frame(-1, 1, 1.2, -45, 6, 1, use_thread=True)
        if drone.control_complete():
            flag = 5


    elif flag == 5 :
        print("状态6: 降落无人机")
        drone.land_auto(use_thread=True)
        if drone.control_complete() :
            flag = 7

    elif flag == 7 :
        print("状态7: 所有动作完成")
        break

    time.sleep(0.1)
